
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       analogin.c
  * @author     baiyang
  * @date       2022-10-25
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "analogin.h"

#include <string.h>
#include <stdio.h>

#include <rtdevice.h>

#include <common/microbee.h>
#include <common/gp_defines.h>
#include <common/time/gp_time.h>
#include <gcs_mavlink/gcs_mavlink.h>
#include <board_config/borad_config.h>

#if defined(HAL_HAVE_ANALOG_INPUT)
#include <board/fmu_analogin_config.h>
#endif

#if defined(HAL_WITH_IO_MCU)
#include <iomcu/gp_iomcu.h>
#endif

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>
/*-----------------------------------macro------------------------------------*/
#define ANLOGIN_DEBUGGING 0

// base voltage scaling for 12 bit 3.3V ADC
#define VOLTAGE_SCALING (3.3f / ((1 << 12) - 1))

// voltage divider is usually 1/(10/(20+10))
#ifndef HAL_IOMCU_VSERVO_SCALAR
  #define HAL_IOMCU_VSERVO_SCALAR 3
#endif

// voltage divider is usually not present
#ifndef HAL_IOMCU_VRSSI_SCALAR
  #define HAL_IOMCU_VRSSI_SCALAR 1
#endif

#if ANLOGIN_DEBUGGING
 # define Debug(fmt, ...)  do {rt_kprintf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ##__VA_ARGS__); } while(0)
#else
 # define Debug(fmt, ...)
#endif

// special pins
#define ANALOG_SERVO_VRSSI_PIN 103

#if defined(HAL_HAVE_ANALOG_INPUT)
#define ADC_GRP1_NUM_CHANNELS   ARRAY_SIZE(analogin_pin_config)
#endif

#if !defined(HAL_ANALOG_PINS)
#define HAL_ANALOG_PINS 0
#endif

#if defined(ADC_RESOLUTION_16B)
// on H7 we use 16 bit ADC transfers, giving us more resolution. We
// need to scale by 1/16 to match the 12 bit scale factors in hwdef.dat
#define ADC_BOARD_SCALING (1.0/16)
#else
#define ADC_BOARD_SCALING 1
#endif

/*----------------------------------typedef-----------------------------------*/
#if defined(HAL_HAVE_ANALOG_INPUT)
struct analogin_pin_info {
    const char dev_name[12];
    rt_adc_device_t analogin_dev;
    const uint8_t channel;
    const float scaling;
};
#endif
/*---------------------------------prototype----------------------------------*/
static void analogsource_ctor(analog_source_t analog_source, int16_t pin);
static analog_source_backend_t analogsource_probe(int16_t pin);
static float _read_average(analog_source_backend_t analog_source_backend);
static float _read_latest(analog_source_backend_t analog_source_backend);
static float _pin_scaler(analog_source_backend_t analog_source_backend);
static float _voltage_average(analog_source_backend_t analog_source_backend);
static float _voltage_average_ratiometric(analog_source_backend_t analog_source_backend);
static float _voltage_latest(analog_source_backend_t analog_source_backend);
static bool _set_pin(analog_source_backend_t analog_source_backend, uint8_t pin);
static void _add_value(analog_source_backend_t analog_source_backend, float v, float vcc5V);
static inline void _read_adc(analogin_t analogin, uint32_t *val);
static void _timer_tick(void *parameter);
static void update_power_flags(analogin_t analogin);

#if defined(ANALOG_VCC_5V_PIN)
static void analogin_publish_power_status(analogin_t analogin);
#endif
/*----------------------------------variable----------------------------------*/
static char thread_analogin_stack[1024];
struct rt_thread thread_analogin_handle;

static struct AnalogSourceBackendOps analog_source_ops = {.read_average = _read_average,
                                                          .read_latest = _read_latest,
                                                          .set_pin = _set_pin,
                                                          .voltage_average = _voltage_average,
                                                          .voltage_latest = _voltage_latest,
                                                          .voltage_average_ratiometric = _voltage_average_ratiometric,};

#if defined(HAL_HAVE_ANALOG_INPUT)
static struct analogin_pin_info analogin_pin_config[] = {HAL_ANALOG_PINS};
#endif

static struct AnalogIn analog_in;

static uitc_power_status power_status;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
static void analogsource_ctor(analog_source_t analog_source, int16_t pin)
{
    analog_source->analog_source_backend.ops = &analog_source_ops;

    analog_source->_pin = pin;

    analog_source->_value = 0.0f;
    analog_source->_value_ratiometric = 0.0f;
    analog_source->_latest_value = 0.0f;
    analog_source->_sum_count = 0;
    analog_source->_sum_value = 0.0f;
    analog_source->_sum_ratiometric = 0.0f;

    char name[RT_NAME_MAX];
    snprintf(name, RT_NAME_MAX, "as%d", pin);
    rt_mutex_init(&analog_source->_mutex, name, RT_IPC_FLAG_FIFO);
}

static analog_source_backend_t analogsource_probe(int16_t pin)
{
    analog_source_t analog_source = (analog_source_t)rt_malloc(sizeof(struct AnalogSource));

    if (!analog_source) {
        return NULL;
    }

    analogsource_ctor(analog_source, pin);

    return (analog_source_backend_t)analog_source;
}

static float _read_average(analog_source_backend_t analog_source_backend)
{
    analog_source_t analog_source = (analog_source_t)analog_source_backend;

    rt_mutex_take(&analog_source->_mutex, RT_WAITING_FOREVER);

    if (analog_source->_sum_count == 0) {
        rt_mutex_release(&analog_source->_mutex);
        return analog_source->_value;
    }
    analog_source->_value = analog_source->_sum_value / analog_source->_sum_count;
    analog_source->_value_ratiometric = analog_source->_sum_ratiometric / analog_source->_sum_count;
    analog_source->_sum_value = 0;
    analog_source->_sum_ratiometric = 0;
    analog_source->_sum_count = 0;

    rt_mutex_release(&analog_source->_mutex);

    return analog_source->_value;
}

static float _read_latest(analog_source_backend_t analog_source_backend)
{
    return ((analog_source_t)analog_source_backend)->_latest_value;
}

/*
  return scaling from ADC count to Volts
 */
static float _pin_scaler(analog_source_backend_t analog_source_backend)
{
    analog_source_t analog_source = (analog_source_t)analog_source_backend;

    float scaling = VOLTAGE_SCALING;

#if defined(HAL_HAVE_ANALOG_INPUT)
    for (uint8_t i=0; i<ADC_GRP1_NUM_CHANNELS; i++) {
        if (analogin_pin_config[i].channel == analog_source->_pin) {
            scaling = analogin_pin_config[i].scaling;
            break;
        }
    }
#endif

    return scaling;
}

/*
  return voltage in Volts
 */
static float _voltage_average(analog_source_backend_t analog_source_backend)
{
    return _pin_scaler(analog_source_backend) * analogsource_read_average(analog_source_backend);
}

/*
  return voltage in Volts, assuming a ratiometric sensor powered by
  the 5V rail
 */
static float _voltage_average_ratiometric(analog_source_backend_t analog_source_backend)
{
    analogsource_voltage_average(analog_source_backend);
    return _pin_scaler(analog_source_backend) * ((analog_source_t)analog_source_backend)->_value_ratiometric;
}

/*
  return voltage in Volts
 */
static float _voltage_latest(analog_source_backend_t analog_source_backend)
{
    return _pin_scaler(analog_source_backend) * analogsource_read_latest(analog_source_backend);
}

static bool _set_pin(analog_source_backend_t analog_source_backend, uint8_t pin)
{
    analog_source_t analog_source = (analog_source_t)analog_source_backend;

    if (analog_source->_pin == pin) {
        return true;
    }
    bool found_pin = false;
    if (pin == ANALOG_SERVO_VRSSI_PIN) {
        found_pin = true;
    } 
#if defined(HAL_HAVE_ANALOG_INPUT)
    else {
        for (uint8_t i=0; i<ADC_GRP1_NUM_CHANNELS; i++) {
            if (analogin_pin_config[i].channel == pin) {
                found_pin = true;
                break;
            }
        }
    }
#endif

    if (!found_pin) {
        return false;
    }

    rt_mutex_take(&analog_source->_mutex, RT_WAITING_FOREVER);

    analog_source->_pin = pin;
    analog_source->_sum_value = 0;
    analog_source->_sum_ratiometric = 0;
    analog_source->_sum_count = 0;
    analog_source->_latest_value = 0;
    analog_source->_value = 0;
    analog_source->_value_ratiometric = 0;

    rt_mutex_release(&analog_source->_mutex);

    return true;
}

/*
  apply a reading in ADC counts
 */
static void _add_value(analog_source_backend_t analog_source_backend, float v, float vcc5V)
{
    analog_source_t analog_source = (analog_source_t)analog_source_backend;

    rt_mutex_take(&analog_source->_mutex, RT_WAITING_FOREVER);

    analog_source->_latest_value = v;
    analog_source->_sum_value += v;
    if (vcc5V < 3.0f) {
        analog_source->_sum_ratiometric += v;
    } else {
        // this compensates for changes in the 5V rail relative to the
        // 3.3V reference used by the ADC.
        analog_source->_sum_ratiometric += v * 5.0f / vcc5V;
    }
    analog_source->_sum_count++;
    if (analog_source->_sum_count == 254) {
        analog_source->_sum_value /= 2;
        analog_source->_sum_ratiometric /= 2;
        analog_source->_sum_count /= 2;
    }

    rt_mutex_release(&analog_source->_mutex);
}

void analogin_init()
{
    if (analog_in._initialized) {
        return;
    }

#if defined(HAL_HAVE_ANALOG_INPUT)
    rt_err_t result = RT_EOK;
    rt_uint8_t resolution = 0;
    rt_int16_t vref = 0;    // ADC采样的参考电压，单位：毫伏。

    for (uint8_t i = 0; i<ADC_GRP1_NUM_CHANNELS; i++) {
        analogin_pin_config[i].analogin_dev = (rt_adc_device_t)rt_device_find(analogin_pin_config[i].dev_name);

        if (analogin_pin_config[i].analogin_dev == NULL) {
            continue;
        }

        if ((((rt_device_t)analogin_pin_config[i].analogin_dev)->open_flag & RT_DEVICE_OFLAG_OPEN) != RT_DEVICE_OFLAG_OPEN) {
            rt_device_open((rt_device_t)analogin_pin_config[i].analogin_dev, RT_DEVICE_OFLAG_RDWR);
        }

        result |= rt_adc_enable(analogin_pin_config[i].analogin_dev, analogin_pin_config[i].channel);
    }

    if (result == RT_EOK) {
        analog_in._initialized = true;
    }
#endif

    rt_mutex_init(&analog_in._mutex, "analogin", RT_IPC_FLAG_FIFO);

    rt_err_t res = rt_thread_init(&thread_analogin_handle,
                           "analogin",
                           _timer_tick,
                           &analog_in,
                           &thread_analogin_stack[0],
                           sizeof(thread_analogin_stack),PRIORITY_ANALOGIN,10);

    RT_ASSERT(res == RT_EOK);

    if (res == RT_EOK) {
        rt_thread_startup(&thread_analogin_handle);
    }
}

analog_source_backend_t analogin_channel(int16_t pin)
{
    rt_mutex_take(&analog_in._mutex, RT_WAITING_FOREVER);

    for (uint8_t j=0; j<ANALOG_MAX_CHANNELS; j++) {
        if (analog_in._channels[j] == NULL) {
            analog_in._channels[j] = analogsource_probe(pin);
            rt_mutex_release(&analog_in._mutex);
            return analog_in._channels[j];
        }
    }

    rt_mutex_release(&analog_in._mutex);

    return NULL;
}

float analogin_board_voltage(void)
{
    return analog_in._board_voltage;
}

float analogin_servorail_voltage(void)
{
    return analog_in._servorail_voltage;
}

uint16_t analogin_power_status_flags(void)
{
    return analog_in._power_flags;
}
uint16_t analogin_accumulated_power_status_flags(void)
{
    return analog_in._accumulated_power_flags;
}

#if defined(HAL_WITH_MCU_MONITORING)
float analogin_mcu_temperature(void) { return analog_in._mcu_temperature; }
float analogin_mcu_voltage(void) { return analog_in._mcu_voltage; }
float analogin_mcu_voltage_max(void) { return analog_in._mcu_voltage_max; }
float analogin_mcu_voltage_min(void) { return analog_in._mcu_voltage_min; }
#endif

/*
  calculate average sample since last read for all channels
 */
static inline void _read_adc(analogin_t analogin, uint32_t *val)
{
#if defined(HAL_HAVE_ANALOG_INPUT)
    for (uint8_t i=0; i<ADC_GRP1_NUM_CHANNELS; i++) {
        if (analogin_pin_config[i].analogin_dev != NULL) {
            val[i] = (uint32_t)rt_adc_read(analogin_pin_config[i].analogin_dev, analogin_pin_config[i].channel);
        }
    }
#else
    RT_UNUSED(analogin);
    RT_UNUSED(val);
#endif
}

/*
  called at 100Hz
 */
static void _timer_tick(void *parameter)
{
    analogin_t analogin = (analogin_t)parameter;

    if (!analogin->_initialized) {
        return;
    }

#if defined(HAL_HAVE_ANALOG_INPUT)
    uint32_t buf_adc[ADC_GRP1_NUM_CHANNELS];
#endif

    while (1) {
        // read adc at 100Hz
        uint32_t now = time_micros();

#if defined(HAL_HAVE_ANALOG_INPUT)
        /* read all channels available */
        _read_adc(analogin, buf_adc);
#endif

        // update power status flags
        update_power_flags(analogin);

#if defined(HAL_HAVE_ANALOG_INPUT)
        // match the incoming channels to the currently active pins
        for (uint8_t i=0; i < ADC_GRP1_NUM_CHANNELS; i++) {
#if defined(ANALOG_VCC_5V_PIN)
            if (analogin_pin_config[i].channel == ANALOG_VCC_5V_PIN) {
                // record the Vcc value for later use in
                // voltage_average_ratiometric()
                analogin->_board_voltage = buf_adc[i] * analogin_pin_config[i].scaling * ADC_BOARD_SCALING;
            }
#endif
#if defined(FMU_SERVORAIL_ADC_CHAN)
            if (analogin_pin_config[i].channel == FMU_SERVORAIL_ADC_CHAN) {
               analogin->_servorail_voltage = buf_adc[i] * analogin_pin_config[i].scaling * ADC_BOARD_SCALING;
            }
#endif
        }
#endif

#if defined(HAL_WITH_IO_MCU)
        // now handle special inputs from IOMCU
        analogin->_servorail_voltage = iomcu_get_vservo_adc_count() * (VOLTAGE_SCALING * HAL_IOMCU_VSERVO_SCALAR);
        analogin->_rssi_voltage = iomcu_get_vrssi_adc_count() * (VOLTAGE_SCALING *  HAL_IOMCU_VRSSI_SCALAR);
#endif

#if defined(HAL_HAVE_ANALOG_INPUT)
        for (uint8_t i=0; i<ADC_GRP1_NUM_CHANNELS; i++) {
            Debug("chan %u value=%u\n",
                  (unsigned)analogin_pin_config[i].channel,
                  (unsigned)buf_adc[i]);
            for (uint8_t j=0; j < ANALOG_MAX_CHANNELS; j++) {
                analog_source_backend_t c = analogin->_channels[j];
                if (c != NULL) {
                    if (analogin_pin_config[i].channel == ((analog_source_t)c)->_pin) {
                        // add a value
                        _add_value(c, buf_adc[i] * ADC_BOARD_SCALING, analogin->_board_voltage);
                    } else if (((analog_source_t)c)->_pin == ANALOG_SERVO_VRSSI_PIN) {
                        _add_value(c, analogin->_rssi_voltage / VOLTAGE_SCALING, 0);
                    }
                }
            }
        }
#endif

#if defined(HAL_WITH_MCU_MONITORING)
        // 20Hz temperature and ref voltage
        static uint32_t last_mcu_temp_us;
        if (now - last_mcu_temp_us > 50000 && brd_is_vehicle_init_finish()) {
            last_mcu_temp_us = now;

            uint32_t buf_adc3[ADC3_GRP1_NUM_CHANNELS];
            uint16_t min_adc3[ADC3_GRP1_NUM_CHANNELS];
            uint16_t max_adc3[ADC3_GRP1_NUM_CHANNELS];

            read_adc3(buf_adc3, min_adc3, max_adc3);

            // factory calibration values
            const float TS_CAL1 = *(const volatile uint16_t *)0x1FF1E820;
            const float TS_CAL2 = *(const volatile uint16_t *)0x1FF1E840;
            const float VREFINT_CAL = *(const volatile uint16_t *)0x1FF1E860;

            analogin->_mcu_temperature = ((110 - 30) / (TS_CAL2 - TS_CAL1)) * ((float)(buf_adc3[1]) - TS_CAL1) + 30;
            analogin->_mcu_voltage = 3.3 * VREFINT_CAL / (float)(buf_adc3[2]+0.001);
            // note min/max swap due to inversion
            analogin->_mcu_voltage_min = 3.3 * VREFINT_CAL / (float)(max_adc3[2]+0.001);
            analogin->_mcu_voltage_max = 3.3 * VREFINT_CAL / (float)(min_adc3[2]+0.001);
        }
#endif

#if defined(ANALOG_VCC_5V_PIN)
        analogin_publish_power_status(analogin);
#endif

        rt_thread_mdelay(10);
    }
}

/*
  update power status flags
 */
static void update_power_flags(analogin_t analogin)
{
    uint16_t flags = 0;

    /*
      primary "brick" power supply valid pin. Some boards have this
      active high, some active low. Use nVALID for active low, VALID
      for active high
    */
#if defined(HAL_GPIO_PIN_VDD_BRICK_VALID)
    if (rt_pin_read(HAL_GPIO_PIN_VDD_BRICK_VALID) == 1) {
        flags |= MAV_POWER_STATUS_BRICK_VALID;
    }
#elif defined(HAL_GPIO_PIN_VDD_BRICK_nVALID)
    if (rt_pin_read(HAL_GPIO_PIN_VDD_BRICK_nVALID) == 0) {
        flags |= MAV_POWER_STATUS_BRICK_VALID;
    }
#endif

    /*
      secondary "brick" power supply valid pin. This is servo rail
      power valid on some boards. Some boards have this active high,
      some active low. Use nVALID for active low, VALID for active
      high. This maps to the MAV_POWER_STATUS_SERVO_VALID in mavlink
      (as this was first added for older boards that used servo rail
      for backup power)
    */
#if defined(HAL_GPIO_PIN_VDD_BRICK2_VALID)
    if (rt_pin_read(HAL_GPIO_PIN_VDD_BRICK_VALID) == 1) {
        flags |= MAV_POWER_STATUS_SERVO_VALID;
    }
#elif defined(HAL_GPIO_PIN_VDD_BRICK2_nVALID)
    if (rt_pin_read(HAL_GPIO_PIN_VDD_BRICK2_nVALID) == 0) {
        flags |= MAV_POWER_STATUS_SERVO_VALID;
    }
#endif

    /*
      USB power. This can be VBUS_VALID, VBUS_nVALID or just
      VBUS. Some boards have both a valid pin and VBUS. The VBUS pin
      is an analog pin that could be used to read USB voltage.
     */
#if defined(HAL_GPIO_PIN_VBUS_VALID)
    if (rt_pin_read(HAL_GPIO_PIN_VBUS_VALID) == 1) {
        flags |= MAV_POWER_STATUS_USB_CONNECTED;
    }
#elif defined(HAL_GPIO_PIN_VBUS_nVALID)
    if (rt_pin_read(HAL_GPIO_PIN_VBUS_nVALID) == 0) {
        flags |= MAV_POWER_STATUS_USB_CONNECTED;
    }
#elif defined(HAL_GPIO_PIN_VBUS)
    if (rt_pin_read(HAL_GPIO_PIN_VBUS) == 1) {
        flags |= MAV_POWER_STATUS_USB_CONNECTED;
    }
#endif

    /*
      overcurrent on "high power" peripheral rail.
     */
#if defined(HAL_GPIO_PIN_VDD_5V_HIPOWER_OC)
    if (rt_pin_read(HAL_GPIO_PIN_VDD_5V_HIPOWER_OC) == 1) {
        flags |= MAV_POWER_STATUS_PERIPH_HIPOWER_OVERCURRENT;
    }
#elif defined(HAL_GPIO_PIN_VDD_5V_HIPOWER_nOC)
    if (rt_pin_read(HAL_GPIO_PIN_VDD_5V_HIPOWER_nOC) == 0) {
        flags |= MAV_POWER_STATUS_PERIPH_HIPOWER_OVERCURRENT;
    }
#endif

    /*
      overcurrent on main peripheral rail.
     */
#if defined(HAL_GPIO_PIN_VDD_5V_PERIPH_OC)
    if (rt_pin_read(HAL_GPIO_PIN_VDD_5V_PERIPH_OC) == 1) {
        flags |= MAV_POWER_STATUS_PERIPH_OVERCURRENT;
    }
#elif defined(HAL_GPIO_PIN_VDD_5V_PERIPH_nOC)
    if (rt_pin_read(HAL_GPIO_PIN_VDD_5V_PERIPH_nOC) == 0) {
        flags |= MAV_POWER_STATUS_PERIPH_OVERCURRENT;
    }
#endif

#if 0
    /*
      this bit of debug code is useful when testing the polarity of
      VALID pins for power sources. It allows you to see the change on
      USB with a 3s delay, so you can see USB changes by unplugging
      and re-inserting USB power
     */
    static uint32_t last_change_ms;
    uint32_t now = time_millis();
    if (analogin->_power_flags != flags) {
        if (last_change_ms == 0) {
            last_change_ms = now;
        } else if (now - last_change_ms > 3000) {
            last_change_ms = 0;
            console_printf("POWR: 0x%02x -> 0x%02x\n", analogin->_power_flags, flags);
            analogin->_power_flags = flags;
        }
        if (brd_get_soft_armed()) {
            // the power status has changed while armed
            flags |= MAV_POWER_STATUS_CHANGED;
        }
        return;
    }
#endif

    if (analogin->_power_flags != 0 &&
        analogin->_power_flags != flags &&
        brd_get_soft_armed()) {
        // the power status has changed while armed
        flags |= MAV_POWER_STATUS_CHANGED;
    }
    analogin->_accumulated_power_flags |= flags;
    analogin->_power_flags = flags;
}

#if defined(ANALOG_VCC_5V_PIN)
static void analogin_publish_power_status(analogin_t analogin)
{
    static uint64_t last_power_status_us = 0;

    uint64_t now = time_micros64();
    if (now - last_power_status_us > 100000) {
        last_power_status_us = now;

        power_status.timestamp_us = now;
        power_status.board_voltage      = analogin->_board_voltage;
        power_status.servorail_voltage  = analogin->_servorail_voltage;
        power_status.power_status_flags = analogin->_power_flags;

        itc_publish(ITC_ID(power_status), &power_status);
    }
}
#endif
/*------------------------------------test------------------------------------*/


